

#include <math.h>
#include "tf_Geom_Algorithm.h"
#include "tf_Element.h"
using namespace std;
/*some function about point begin*/

double GetTwoPointDis(double pt1[3],double pt2[3])
{
	double dist;
	double vec[3];
	
	vec[0] = pt1[0] - pt2[0];
	vec[1] = pt1[1] - pt2[1];
	vec[2] = pt1[2] - pt2[2];
	
	dist = sqrt( vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2] );
	
	return dist;
}

double GetTwoPointDis(TfPoint pt1,TfPoint pt2)
{
	double pta[3],ptb[3];
	pt1.GetPoint(pta);
	pt2.GetPoint(ptb);
	return GetTwoPointDis(pta,ptb);
}
/*some function about point end*/



/*some function about line begin*/

// double GetLineLength(TfLine &Line)
// {
// 	vector<TfPoint> ptVec;
// 	double dis = 0.0;
// 	int i;
// 	ptVec = Line.GetLinePtVec();
// 	for (i = 0;i < ptVec.size()-1;i++)
// 	{
// 		dis += GetTwoPointDis(ptVec[i],ptVec[i+1]);
// 	}
// 	return dis;
// }



/*some function about line begin*/



/*other functions*/
double GetAngleFromSinVCosV(double sinV,double cosV)
{
	double angleV;
	angleV = asin(fabs(sinV));
	angleV = 180.0*angleV/PI;
	
	if(fabs(sinV-1.0) < EPSS)
		return 90.0;
	else if (fabs(sinV+1.0) < EPSS)
		return -90;
	else if(fabs(cosV-1.0) < EPSS)
		return 0.0;
	else if(fabs(cosV+1.0) < EPSS)
		return 180.0;
	else if(sinV > 0&& cosV > 0)
		return angleV;
	else if(sinV > 0&& cosV <0)
		return 180-angleV;
	else if(sinV < 0&& cosV >0)
		return -angleV;
	else if(sinV < 0&& cosV <0)
		return -(180-angleV);
	
	return 0.0;
	
}
/*some function about vector*/
void UnitVector(double v[3])
{
	double dist;
	
    dist = sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
	v[0] = v[0] / dist;
	v[1] = v[1] / dist;
	v[2] = v[2] / dist;

}

//Matrix

void InitMatrix(double matrix[4][4])
{
	memset(matrix,0,sizeof(double)*4*4);
	matrix[0][0] = matrix[1][1] = matrix[2][2] = matrix[3][3] = 1.0;
}
void PointPlusMatrix(double pt[3],double matrix[4][4],double ptResult[3])
{
		memcpy(ptResult,pt,3*sizeof(double));
		pt[0] = (ptResult[0]*matrix[0][0]+ptResult[1]*matrix[1][0]+ptResult[2]*matrix[2][0]+matrix[3][0]);
		pt[1] = (ptResult[0]*matrix[0][1]+ptResult[1]*matrix[1][1]+ptResult[2]*matrix[2][1]+matrix[3][1]);
		pt[2] = (ptResult[0]*matrix[0][2]+ptResult[1]*matrix[1][2]+ptResult[2]*matrix[2][2]+matrix[3][2]);

}
void InverseMatrix(double a[], int n)
{
		int *is, *js, i, j, k, l, u, v; 
	double d, p; 

	is = new int[n];
	js = new int[n];
	for (k = 0; k <= n-1; k++) 
	{ 
		d = 0.0; 
		for (i = k; i <= n-1; i++) 
			for (j = k; j <= n-1; j++) 
			{
				l = i * n + j; 
				p = fabs(a[l]); 
				if (p > d) 
				{ 
					d = p; 
					is[k] = i; 
					js[k] = j;
				} 
			} 

		if (d+1.0 == 1.0)
		{ 
			delete []is;is=NULL;
			delete []js;js=NULL; 
			return; 
		} 

		if (is[k] != k) 
		{
			for (j = 0; j <= n-1; j++) 
			{ 
				u = k * n + j;
				v = is[k] * n + j;
				p = a[u]; 
				a[u] = a[v]; 
				a[v] = p; 
			} 
		}
			
		if (js[k] != k) 
		{
			for (i = 0; i <= n-1; i++) 
			{ 
				u = i * n + k; 
				v = i * n + js[k]; 
				p = a[u]; 
				a[u] = a[v]; 
				a[v] = p; 
			} 
		}

		l = k * n + k; 
		a[l] = 1.0 / a[l]; 

		for (j = 0; j <= n-1; j++) 
		{
			if (j != k) 
			{ 
				u = k * n + j; 
				a[u] = a[u] * a[l];
			} 
		}

		for (i = 0; i <= n-1; i++)
		{
			if (i != k) 
			{
				for (j = 0; j <= n-1; j++) 
				{
					if (j != k) 
					{
						u = i * n + j; 
						a[u] = a[u] - a[i*n+k] * a[k*n+j]; 
					} 
				}
			}
		}
 
		for (i = 0; i <= n-1; i++)
		{
			if (i != k) 
			{ 
				u = i * n + k; 
				a[u] = -a[u] * a[l];
			} 
		}
	} 

	for (k = n-1; k >= 0; k--) 
	{
		if (js[k] != k) 
		{
			for (j = 0; j <= n-1; j++) 
			{
				u = k * n + j;
				v = js[k] * n + j; 
				p = a[u];
				a[u] = a[v]; 
				a[v] = p; 
			} 
		}
		if (is[k] != k) 
		{
			for (i = 0; i <= n-1; i++) 
			{ 
				u = i * n + k;
				v = i * n + is[k];
				p = a[u];
				a[u] = a[v]; 
				a[v] = p; 
			} 
		}
	} 
	delete []is;is=NULL; 
	delete []js;js=NULL; 
}
void Martrix4444(double a[][4],double b[][4],double c[][4])
{
	int i,j,t;
	for(i=0;i<4;i++)                                          
		for(j=0;j<4;j++)  
		{          
			c[i][j]=0;                                     
			for(t=0;t<4;t++)  
				c[i][j]=c[i][j]+a[i][t]*b[t][j];  
		}  
		
}
void CopyMartrix(double matrixa[4][4],double matrixb[4][4] )/*a = b*/
{
	for (int m = 0; m <4; m++)
	{
		for (int n = 0 ; n < 4;n++)
		{
			matrixa[m][n] = matrixb[m][n];
		}
	}
	
}


int GetModelsBox(double &minX,double &minY,double &minZ,double &maxX,double &maxY,double &maxZ)
{
	int i,j;
	double pt[3];


	int isHaveModel = -1;
	vector<int> partLineVec;
	vector<int> ndLineVec;
	minX = minY = minZ = HUG_VALUE;
	maxX = maxY = maxZ = -HUG_VALUE;
	TfLinMng.GetPartLines(1,partLineVec);
	for (i = 0; i < partLineVec.size();i++)
	{
		TfLinMng.GetLineNodes(partLineVec[i],ndLineVec);
		for (j = 0; j < ndLineVec.size();j++)
		{
			TfNdMng.GetNodePoint(ndLineVec[j],pt);
			if(pt[0] < minX)  minX = pt[0];
			if(pt[0] > maxX)  maxX = pt[0];
			if(pt[1] < minY)  minY = pt[1];
			if(pt[1] > maxY)  maxY = pt[1];
			if(pt[2] < minZ)  minZ = pt[2];
			if(pt[2] > maxZ)  maxZ = pt[2];
		}	
		ndLineVec.clear();
		isHaveModel = 1;
	}



	vector<int> partEleVec;
	vector<int> ndEleVec;
	TfEmMng.GetPartElements(1,partEleVec);
	for (i = 0; i < partEleVec.size();i++)
	{
		TfEmMng.GetElementNodes(partEleVec[i],ndEleVec);
		for (j = 0; j < ndEleVec.size();j++)
		{
			TfNdMng.GetNodePoint(ndEleVec[j],pt);
			if(pt[0] < minX)  minX = pt[0];
			if(pt[0] > maxX)  maxX = pt[0];
			if(pt[1] < minY)  minY = pt[1];
			if(pt[1] > maxY)  maxY = pt[1];
			if(pt[2] < minZ)  minZ = pt[2];
			if(pt[2] > maxZ)  maxZ = pt[2];
		}
		ndEleVec.clear();
		isHaveModel = 1;
	}
	
	return isHaveModel;
}

